#include "ugv_sdk/scout/scout_base.hpp"
#include <iostream>
#include <iomanip>
#include <unistd.h>
#include <termios.h>
#include <ctime>      // 用于获取当前时间
#include <cmath>      // 用于获取当前时间
#include <clocale>    // 用于中文显示

using namespace westonrobot;

// 【对齐参考代码】ANSI终端控制码（含清屏、多颜色、样式）
#define CLR_RESET "\033[0m"
#define CLR_RED "\033[31;1m"
#define CLR_GREEN "\033[32;1m"
#define CLR_YELLOW "\033[33;1m"
#define CLR_BLUE "\033[34;1m"
#define CLR_PURPLE "\033[35;1m"
#define CLR_CYAN "\033[36;1m"
#define STYLE_BOLD "\033[1m"
#define CLEAR_SCREEN "\033[2J\033[H"  // 清屏+光标归位，避免信息堆积

// 【对齐参考代码】装饰性分隔符
const char* LINE_TOP = "==============================================";
const char* LINE_MID = "----------------------------------------------";

// 终端设置：支持按Q键退出（保留原功能，优化命名）
struct termios original_termios;
void disable_terminal_buffering() {
    tcgetattr(STDIN_FILENO, &original_termios);
    struct termios new_termios = original_termios;
    new_termios.c_lflag &= ~(ICANON | ECHO);  // 禁用行缓冲+回显
    new_termios.c_cc[VMIN] = 0;               // 非阻塞读取（不等待按键）
    new_termios.c_cc[VTIME] = 0;
    tcsetattr(STDIN_FILENO, TCSANOW, &new_termios);
}

void restore_terminal() {
    tcsetattr(STDIN_FILENO, TCSANOW, &original_termios);
}

// 【新增】获取当前时间字符串（对齐参考代码的"最后更新"）
std::string get_current_time() {
    time_t now = time(nullptr);
    struct tm* local_tm = localtime(&now);
    char time_buf[64];
    strftime(time_buf, sizeof(time_buf), "%Y-%m-%d %H:%M:%S", local_tm);
    return time_buf;
}

int main(int argc, char **argv) {
    // 【对齐参考代码】设置中文locale，避免乱码
    setlocale(LC_CTYPE, "zh_CN.utf8");

    // 【完善功能】终端初始化+小车连接（增加连接失败判断）
    disable_terminal_buffering();
    bool running = true;
    ScoutBase scout;
    int32_t baud_rate = 0;
    scout.Connect("can0", baud_rate);

    while (running) {
        // 1. 【对齐参考代码】清屏刷新，避免信息堆积
        std::cout << CLEAR_SCREEN;

        // 2. 获取小车状态
        auto state = scout.GetScoutState();

        // 3. 【对齐参考代码】标题栏（青色粗体+居中）
        std::cout << CLR_CYAN << LINE_TOP << CLR_RESET << std::endl;
        std::cout << CLR_CYAN << STYLE_BOLD << "          松灵小车状态监控面板          " << CLR_RESET << std::endl;
        std::cout << CLR_CYAN << LINE_TOP << CLR_RESET << "\n";

        // 4. 【对齐参考代码】模块1：灯光系统（蓝色组）
        std::string light_mode_str;
        switch (state.front_light_state.mode) {
            case LIGHT_MODE_BREATH:    light_mode_str = "常亮模式"; break;
            case LIGHT_MODE_CONST_ON:  light_mode_str = "常亮模式"; break;
            case LIGHT_MODE_CONST_OFF: light_mode_str = "关闭模式"; break;
            case LIGHT_MODE_CUSTOM:    light_mode_str = "可编程模式"; break;
            default:                   light_mode_str = "未知模式";
        }
        std::cout << CLR_BLUE << STYLE_BOLD << "[灯光系统状态]" << CLR_RESET << std::endl;
        std::cout << "  " << CLR_BLUE << "模式: " << STYLE_BOLD << light_mode_str << CLR_RESET << "\n";

        // 5. 【对齐参考代码】模块2：电源系统（绿色组+低电量预警）
        std::cout << CLR_GREEN << STYLE_BOLD << "[电源系统状态]" << CLR_RESET << std::endl;
        // 电池电压预警：低于11.5V标红+警告符号
        const char* bat_color = (state.battery_voltage < 11.5) ? CLR_RED : CLR_GREEN;
        std::cout << "  " << CLR_GREEN << "电池电压: " << bat_color << std::fixed << std::setprecision(2) 
                  << state.battery_voltage << "V" << (state.battery_voltage < 11.5 ? "  ⚠️ 低电量预警" : "") 
                  << CLR_RESET << std::endl;
        // 驱动器电压（取第一个电机的驱动器电压作为参考）
        if (state.motor_num > 0) {
            std::cout << "  " << CLR_GREEN << "驱动器电压: " << std::fixed << std::setprecision(2) 
                      << state.actuator_states[0].driver_voltage << "V" << CLR_RESET << std::endl;
        }
        std::cout << "\n";

        // 6. 【对齐参考代码】模块3：动力系统（黄色组+电机过热预警）
        std::cout << CLR_YELLOW << STYLE_BOLD << "[动力系统状态]" << CLR_RESET << std::endl;
        std::cout << "  " << CLR_YELLOW << "电机数量: " << static_cast<int>(state.motor_num) << "个" << CLR_RESET << std::endl;
        // 遍历所有电机，逐个显示状态+过热预警（温度>55℃标红）
        for (size_t i = 0; i < state.motor_num; ++i) {
            auto& motor = state.actuator_states[i];
            const char* temp_color = (motor.motor_temperature > 55) ? CLR_RED : CLR_YELLOW;
            std::cout << "  " << CLR_YELLOW << "电机" << i << ": " 
                      << "电流=" << std::fixed << std::setprecision(2) << motor.motor_current << "A, "
                      << "转速=" << motor.motor_rpm << "RPM, "
                      << "温度=" << temp_color << motor.motor_temperature << "℃" 
                      << (motor.motor_temperature > 55 ? "  ⚠️ 过热预警" : "") 
                      << CLR_RESET << std::endl;
        }
        std::cout << "\n";

        // 7. 【对齐参考代码】模块4：驱动控制（紫色组）
        std::cout << CLR_PURPLE << STYLE_BOLD << "[驱动控制状态]" << CLR_RESET << std::endl;
        if (state.motor_num > 0) {
            auto& driver = state.actuator_states[0];
            // 驱动器状态：0=正常，其他=异常
            const char* driver_state_str = (driver.driver_state == 0) ? CLR_GREEN"正常" : CLR_RED"异常";
            std::cout << "  " << CLR_PURPLE << "驱动器状态: " << driver_state_str << CLR_RESET << std::endl;
            std::cout << "  " << CLR_PURPLE << "驱动器温度: " << driver.driver_temperature << "℃" << CLR_RESET << std::endl;
        }
        std::cout << "\n";

        // 8. 【对齐参考代码】模块5：运动性能（青色组）
        std::cout << CLR_CYAN << STYLE_BOLD << "[运动性能状态]" << CLR_RESET << std::endl;
        // 线速度：正=前进，负=后退
        std::string linear_dir = (state.linear_velocity >= 0) ? "前进" : "后退";
        std::cout << "  " << CLR_CYAN << "线速度: " << linear_dir << " " << std::fixed << std::setprecision(2) 
                  << fabs(state.linear_velocity) << "m/s" << CLR_RESET << std::endl;
        // 角速度：正=逆时针（左转），负=顺时针（右转）
        std::string angular_dir = (state.angular_velocity >= 0) ? "逆时针（左转）" : "顺时针（右转）";
        std::cout << "  " << CLR_CYAN << "角速度: " << angular_dir << " " << std::fixed << std::setprecision(2) 
                  << fabs(state.angular_velocity) << "rad/s" << CLR_RESET << "\n";

        // 9. 【对齐参考代码】底部信息（分隔线+更新时间+操作提示）
        std::cout << CLR_CYAN << LINE_MID << CLR_RESET << std::endl;
        std::cout << STYLE_BOLD << "最后更新: " << get_current_time() << CLR_RESET << std::endl;
        std::cout << STYLE_BOLD << "操作提示: 按 Q 键退出程序" << CLR_RESET << std::endl;
        std::cout << CLR_CYAN << LINE_MID << CLR_RESET << std::endl;

        // 10. 检查退出按键（非阻塞，不影响刷新）
        char key;
        if (read(STDIN_FILENO, &key, 1) > 0) {
            if (key == 'q' || key == 'Q') running = false;
        }

        // 刷新间隔：500ms（与原代码一致，避免频繁刷新）
        usleep(500000);
    }

    // 【完善清理】退出时恢复终端+断开连接+清屏提示
    std::cout << CLEAR_SCREEN;
    restore_terminal();
    scout.Disconnect();
    std::cout << CLR_GREEN << STYLE_BOLD << "程序已安全退出，小车连接已断开" << CLR_RESET << std::endl;

    return 0;
}
